/*
 * SimpleCDMOTOR Arduino Library
 * Stepper Motor Control via CDBUS/CDNET
 * 
 * Copyright (c) 2024
 * Licensed under MIT License
 * Based on cdstep motor control implementation
 */

#ifndef SIMPLE_CD_MOTOR_H
#define SIMPLE_CD_MOTOR_H

#include <Arduino.h>
#include "SimpleCDNET.h"

// Motor states
#define MOTOR_STATE_DISABLED  0
#define MOTOR_STATE_ENABLED   1
#define MOTOR_STATE_MOVING    2
#define MOTOR_STATE_HOMING    3

// Control modes
#define CONTROL_MODE_POSITION 0
#define CONTROL_MODE_VELOCITY 1

// Motor configuration structure
typedef struct {
    uint8_t enable_pin;
    uint8_t dir_pin;
    uint8_t step_pin;
    uint8_t ms1_pin;
    uint8_t ms2_pin;
    uint8_t ms3_pin;
    uint8_t home_pin;
    uint8_t limit_pin;
    
    // Motor parameters
    uint16_t steps_per_rev;
    uint16_t max_speed;
    uint16_t max_accel;
    uint16_t current_limit;
    
    // PID parameters
    float kp;
    float ki;
    float kd;
} motor_config_t;

// Motor state structure
typedef struct {
    int32_t current_pos;
    int32_t target_pos;
    int32_t current_speed;
    int32_t target_speed;
    
    uint8_t state;
    uint8_t control_mode;
    bool homed;
    bool limit_reached;
    
    // Trajectory planning
    int32_t tc_pos;
    int32_t tc_speed;
    int32_t tc_accel;
    
    // PID controller
    float pid_output;
    float pid_integral;
    float pid_last_error;
} motor_state_t;

class SimpleCDMOTOR {
private:
    SimpleCDNET _cdnet;
    motor_config_t _config;
    motor_state_t _state;
    
    // CDNET port for motor control
    uint16_t _motor_port;
    uint8_t _motor_addr;
    
    // Timing
    uint32_t _last_update;
    uint32_t _update_interval;
    
public:
    SimpleCDMOTOR();
    
    // Initialization
    void begin(HardwareSerial *serial, uint8_t address, uint16_t motor_port);
    void configure(const motor_config_t &config);
    
    // Basic motor control
    void enable();
    void disable();
    void setPosition(int32_t position);
    void setVelocity(int32_t velocity);
    void home();
    void stop();
    
    // Status
    bool isEnabled();
    bool isMoving();
    bool isHomed();
    int32_t getPosition();
    int32_t getTargetPosition();
    uint8_t getState();
    
    // Configuration
    void setMaxSpeed(uint16_t max_speed);
    void setMaxAccel(uint16_t max_accel);
    void setPID(float kp, float ki, float kd);
    
    // Update - call in loop
    void update();
    
    // CDNET commands
    void sendMotorCommand(uint8_t command, int32_t value);
    void processMotorPacket(uint8_t *data, uint8_t len);
    
private:
    void initializeMotor();
    void updateTrajectory();
    void updatePID();
    void sendStepCommand(int32_t steps);
    void updateLimitSwitches();
};

#endif // SIMPLE_CD_MOTOR_H